#ifndef _CHASSIS_H
#define _CHASSIS_H

#include "base_drv/rc.h"
#include "base_drv/motor_ctrl.h"

#define S_CURVE_VX_ACC 5.0f
#define S_CURVE_VY_ACC 4.7f


#define MAX_VX_SPEED 3.5f             //3.5f
#define MAX_VY_SPEED 3.2f
#define CONSTANT_SPINNING_SPEED 40.0f

#define SHUT_DEADBAND 600



typedef enum{
	CHASSIS_FOLLOW = 0,
	CHASSIS_ISOLATED = 1,
	SPINNING_PENDING_SHUT = 3,
	SPINNING = 2,
}chassis_spin_t;

typedef __packed struct
{
    uint8_t stop_ready;
	
}chassis_command_t;

void chassis_init(void);
void chassis_control_loop(rc_ctrl_t* rc_recv);

void reset_chassis_angle(void);
int16_t get_gimbal_angle(motor_response_msg_t chassis_motors[4] ,motor_response_msg_t* gimbal_motor);
float get_spinning_speed(void);
float get_current_chassis_wz(void);

#endif

